#include "ContoursPostprocessing.h"
#include <numeric>
#include <algorithm>
#include <cmath>

namespace ContourProcessing {

// 颜色转换实现
Gray16 ColorUtils::rgbaToGray16(const ColorRGBA& color) {
    float grayFloat = color.x() * 0.299f + color.y() * 0.587f + color.z() * 0.114f;
    return static_cast<Gray16>(std::clamp<double>(std::round(grayFloat * GRAY16_MAX), 0.0, static_cast<double>(GRAY16_MAX)));
}

void ColorUtils::rgbToHSV(float r, float g, float b, float& h, float& s, float& v) {
    float max = std::max({r, g, b});
    float min = std::min({r, g, b});
    float delta = max - min;

    v = max;

    if (delta < 1e-5f) {
        h = 0.0f;
        s = 0.0f;
        return;
    }

    s = delta / max;

    if (max == r) {
        h = (g - b) / delta;
    } else if (max == g) {
        h = 2.0f + (b - r) / delta;
    } else {
        h = 4.0f + (r - g) / delta;
    }

    h *= 60.0f;
    if (h < 0.0f) h += 360.0f;
    h /= 360.0f; // 归一化到 [0,1]
}

Gray16 ColorUtils::rgbaToHSV_V_16bit(const ColorRGBA& color) {
    float h, s, v;
    rgbToHSV(color.x(), color.y(), color.z(), h, s, v);
    return static_cast<Gray16>(std::clamp<double>(std::round(v * GRAY16_MAX), 0.0, static_cast<double>(GRAY16_MAX)));
}

ColorRGBA ColorUtils::binaryToRGBA(Gray16 binaryValue) {
    if (binaryValue == GRAY16_MAX) {
        return ColorRGBA(1.0f, 1.0f, 1.0f, 1.0f); // 白色
    } else {
        return ColorRGBA(0.0f, 0.0f, 0.0f, 1.0f); // 黑色
    }
}

// 数学工具实现
float MathUtils::distance3D(const Point3D& p1, const Point3D& p2) {
    return std::sqrt(std::pow(p2.x() - p1.x(), 2) +
                     std::pow(p2.y() - p1.y(), 2) +
                     std::pow(p2.z() - p1.z(), 2));
}

Point3D MathUtils::interpolatePoints(const Point3D& p1, const Point3D& p2, float t) {
    return p1 + (p2 - p1) * t;
}

// ContourProcessor 成员函数实现
Gray16 ContourProcessor::rgbaToGray16(const ColorRGBA& color) const {
    return ColorUtils::rgbaToGray16(color);
}

Gray16 ContourProcessor::rgbaToHSV_V_16bit(const ColorRGBA& color) const {
    return ColorUtils::rgbaToHSV_V_16bit(color);
}

Gray16 ContourProcessor::rgbaToHybridIntensity(const ColorRGBA& color) const {
    Gray16 gray = rgbaToGray16(color);
    Gray16 v_channel = rgbaToHSV_V_16bit(color);
    return static_cast<Gray16>((static_cast<float>(gray) * static_cast<float>(v_channel)) / GRAY16_MAX);
}

// 模板特化：强度值计算
template<>
Gray16 ContourProcessor::calculateIntensity<InterpolationMode::Gray>(const ColorRGBA& color) const {
    return rgbaToGray16(color);
}

template<>
Gray16 ContourProcessor::calculateIntensity<InterpolationMode::VChannel>(const ColorRGBA& color) const {
    return rgbaToHSV_V_16bit(color);
}

template<>
Gray16 ContourProcessor::calculateIntensity<InterpolationMode::Hybrid>(const ColorRGBA& color) const {
    return rgbaToHybridIntensity(color);
}

Gray16 ContourProcessor::gray16ToBinary(Gray16 grayValue, Gray16 threshold, bool invert) const {
    threshold = std::clamp(threshold, static_cast<Gray16>(0), GRAY16_MAX);
    if (!invert) {
        return (grayValue >= threshold) ? GRAY16_MAX : 0;
    } else {
        return (grayValue <= threshold) ? GRAY16_MAX : 0;
    }
}

int ContourProcessor::calculateInterpolationCount(float distance, Gray16 intensityValue,
                                                  float maxDistance, Gray16 maxIntensity,
                                                  int baseCount, float distanceWeight,
                                                  float grayWeight) const {
    if (maxDistance == 0.0f || maxIntensity == 0) {
        return baseCount;
    }

    // 归一化距离和强度值
    float normDistance = MathUtils::normalize(distance, maxDistance);
    float normGray = MathUtils::normalize(static_cast<float>(intensityValue),
                                          static_cast<float>(maxIntensity));

    // 计算插值分数（非线性关系）
    float distanceScore = MathUtils::power(normDistance, 2.0f);
    float grayScore = MathUtils::power(normGray, 1.5f);

    // 加权综合得分
    float totalScore = distanceWeight * distanceScore + grayWeight * grayScore;

    // 根据得分确定插值数量
    if (totalScore < 0.2f) {
        return baseCount;
    } else if (totalScore < 0.4f) {
        return baseCount + 1;
    } else if (totalScore < 0.6f) {
        return baseCount + 2;
    } else if (totalScore < 0.8f) {
        return baseCount + 3;
    } else {
        return baseCount + 4;
    }
}

QVector<std::pair<Point3D, Gray16>>
ContourProcessor::insertPointsForHighIntensity(const QVector<Point3D>& points,
                                               const QVector<Gray16>& binaryColors,
                                               const QVector<Gray16>& intensityValues,
                                               const GlobalStats& stats,
                                               const ProcessingParams& params) const {
    if (points.size() < 2) {
        QVector<std::pair<Point3D, Gray16>> result;
        for (int i = 0; i < points.size(); ++i) {
            result.append({points[i], binaryColors[i]});
        }
        return result;
    }

    QVector<std::pair<Point3D, Gray16>> interpolatedPoints;

    // 添加所有原始点
    for (int i = 0; i < points.size(); ++i) {
        interpolatedPoints.append({points[i], binaryColors[i]});
    }

    // 在相邻高强度点之间插入多个点
    for (int i = 0; i < points.size() - 1; ++i) {
        const Point3D& currentPoint = points[i];
        Gray16 currentBinary = binaryColors[i];
        Gray16 currentIntensity = intensityValues[i];
        const Point3D& nextPoint = points[i + 1];
        Gray16 nextBinary = binaryColors[i + 1];
        Gray16 nextIntensity = intensityValues[i + 1];
        // 只在两个高强度点之间进行插值
        if (currentBinary == GRAY16_MAX && nextBinary == GRAY16_MAX) {
            float distance = MathUtils::distance3D(nextPoint, currentPoint);
            float avgIntensity = (static_cast<float>(currentIntensity) +
                                  static_cast<float>(nextIntensity)) / 2.0f;
            // 计算插值数量
            int numInterpolations = calculateInterpolationCount(
                distance, static_cast<Gray16>(avgIntensity),
                stats.maxDistance, stats.maxIntensity,
                params.higherBaseCount, params.higherDistanceWeight,
                params.higherIntensityWeight
                );
            // 执行均匀线性插值
            for (int j = 1; j <= numInterpolations; ++j) {
                float t = static_cast<float>(j) / (numInterpolations + 1);
                Point3D interpolatedPoint = MathUtils::interpolatePoints(currentPoint, nextPoint, t);
                interpolatedPoints.append({interpolatedPoint, GRAY16_MAX});
            }
        }
    }

    return interpolatedPoints;
}

QVector<std::pair<Point3D, Gray16>>
ContourProcessor::removeLowIntensityPoints(const QVector<Point3D>& points,
                                           const QVector<Gray16>& binaryColors,
                                           float removalRatio) const {
    if (points.isEmpty()) {
        return {};
    }

    // 分离低强度点和高强度点
    QVector<int> lowIntensityIndices;
    QVector<int> highIntensityIndices;

    for (int i = 0; i < binaryColors.size(); ++i) {
        if (binaryColors[i] == 0) {
            lowIntensityIndices.append(i);
        } else {
            highIntensityIndices.append(i);
        }
    }

    if (lowIntensityIndices.isEmpty()) {
        QVector<std::pair<Point3D, Gray16>> result;
        for (int i = 0; i < points.size(); ++i) {
            result.append({points[i], binaryColors[i]});
        }
        return result;
    }

    // 计算需要保留的低强度点数量
    int numLowToKeep = std::max(1, static_cast<int>(lowIntensityIndices.size() * (1.0f - removalRatio)));

    QVector<int> lowIndicesToKeep;
    if (numLowToKeep < lowIntensityIndices.size()) {
        // 模拟np.linspace(0, len-1, numLowToKeep)的行为
        int totalSteps = lowIntensityIndices.size() - 1; // 区间终点（和Python一致）

        if (numLowToKeep == 1) {
            // 当只需要1个索引时，返回起始点0（匹配Python的linspace）
            lowIndicesToKeep.append(lowIntensityIndices[0]);
        } else {
            // 当需要多个索引时，均匀分布计算（避免除以零）
            for (int i = 0; i < numLowToKeep; ++i) {
                // 计算比例：i / (numLowToKeep - 1) → 范围[0,1]
                float ratio = static_cast<float>(i) / (numLowToKeep - 1);
                // 映射到总区间[0, totalSteps]，四舍五入取整
                int index = static_cast<int>(std::round(ratio * totalSteps));
                // 确保索引在有效范围内（防御性处理）
                index = std::clamp(index, 0, totalSteps);
                lowIndicesToKeep.append(lowIntensityIndices[index]);
            }
        }
    } else {
        lowIndicesToKeep = lowIntensityIndices;
    }

    // 合并要保留的所有点索引
    QVector<int> allIndicesToKeep = highIntensityIndices + lowIndicesToKeep;
    std::sort(allIndicesToKeep.begin(), allIndicesToKeep.end());

    QVector<std::pair<Point3D, Gray16>> result;
    for (int index : allIndicesToKeep) {
        result.append({points[index], binaryColors[index]});
    }

    return result;
}

QVector<Vertex> ContourProcessor::processSingleContour(const QVector<Vertex>& contour,
                                                       const GlobalStats& stats,
                                                       const ProcessingParams& params,
                                                       ProcessingStats& sliceStats) {
    if (contour.isEmpty()) {
        return {};
    }
    // 提取点和颜色
    QVector<Point3D> points;
    QVector<ColorRGBA> colors;
    for (const Vertex& vertex : contour) {
        points.append(vertex.pos);
        colors.append(vertex.color);
    }

    sliceStats.totalOriginalPoints += points.size();

    // 根据模式计算强度值
    QVector<Gray16> intensityValues;
    switch (params.interpMode) {
    case InterpolationMode::Gray:
        for (const ColorRGBA& color : colors) {
            intensityValues.append(calculateIntensity<InterpolationMode::Gray>(color));
        }
        break;
    case InterpolationMode::VChannel:
        for (const ColorRGBA& color : colors) {
            intensityValues.append(calculateIntensity<InterpolationMode::VChannel>(color));
        }
        break;
    case InterpolationMode::Hybrid:
        for (const ColorRGBA& color : colors) {
            intensityValues.append(calculateIntensity<InterpolationMode::Hybrid>(color));
        }
        break;
    }

    // 二值化
    QVector<Gray16> binaryColors;
    for (Gray16 intensity : intensityValues) {
        binaryColors.append(gray16ToBinary(intensity, params.binaryThreshold, params.binaryInvert));
    }

    // 统计原始高低强度点数量
    int originalLowCount = std::count(binaryColors.begin(), binaryColors.end(), 0);
    int originalHighCount = std::count(binaryColors.begin(), binaryColors.end(), GRAY16_MAX);
    sliceStats.originalLowerPoints += originalLowCount;
    sliceStats.originalHigherPoints += originalHighCount;

    // 高强度点插值
    QVector<std::pair<Point3D, Gray16>> interpolatedPairContainer = insertPointsForHighIntensity(
        points, binaryColors, intensityValues, stats, params);

    // 统计插值新增的点数
    int higherAdded = interpolatedPairContainer.size() - points.size();
    sliceStats.totalHigherAdded += higherAdded;

    // 低强度点剔除
    QVector<Point3D> finalPoints;
    QVector<Gray16> finalBinary;
    for (const auto& [point, binary] : interpolatedPairContainer) {
        finalPoints.append(point);
        finalBinary.append(binary);
    }

    auto finalResult = removeLowIntensityPoints(finalPoints, finalBinary, params.lowerRemovalRatio);

    // 统计剔除的低强度点数量
    int finalLowCount = std::count_if(finalResult.begin(), finalResult.end(),
                                      [](const auto& pair) { return pair.second == 0; });
    int lowerRemoved = originalLowCount - finalLowCount;
    sliceStats.totalLowerRemoved += lowerRemoved;

    // 转换回Vertex格式
    QVector<Vertex> resultVertices;
    for (const auto& [point, binary] : finalResult) {
        Vertex vertex;
        vertex.pos = point;
        vertex.color = ColorRGBA(0.0f, 0.0f, 0.0f, 1.0f);/*ColorUtils::binaryToRGBA(binary)*/;
        vertex.norm = QVector3D(0, 0, 1); // 默认法向量
        resultVertices.append(vertex);
    }

    sliceStats.totalProcessedPoints += resultVertices.size();
    return resultVertices;
}

GlobalStats ContourProcessor::calculateGlobalStats(const ContourData& contoursData,
                                                   InterpolationMode mode) {
    GlobalStats stats;
    stats.totalSlices = contoursData.size();
    stats.mode = mode;

    QVector<float> allDistances;

    for (const Slice& slice : contoursData) {
        stats.totalContours += slice.size();

        for (const QVector<Vertex>& contour : slice) {
            stats.totalPoints += contour.size();

            if (contour.isEmpty()) continue;

            // 统计强度最大值
            for (const Vertex& vertex : contour) {
                Gray16 intensity = 0;
                switch (mode) {
                case InterpolationMode::Gray:
                    intensity = ColorUtils::rgbaToGray16(vertex.color);
                    break;
                case InterpolationMode::VChannel:
                    intensity = ColorUtils::rgbaToHSV_V_16bit(vertex.color);
                    break;
                case InterpolationMode::Hybrid:
                    intensity = (ColorUtils::rgbaToGray16(vertex.color) *
                                 ColorUtils::rgbaToHSV_V_16bit(vertex.color)) / GRAY16_MAX;
                    break;
                }

                if (mode == InterpolationMode::Gray) {
                    stats.maxGray = std::max(stats.maxGray, intensity);
                } else if (mode == InterpolationMode::VChannel) {
                    stats.maxV = std::max(stats.maxV, intensity);
                }
                stats.maxIntensity = std::max(stats.maxIntensity, intensity);
            }

            // 统计最大距离
            if (contour.size() >= 2) {
                for (int i = 0; i < contour.size(); ++i) {
                    const Point3D& currPoint = contour[i].pos;
                    const Point3D& nextPoint = contour[(i + 1) % contour.size()].pos;
                    float distance = MathUtils::distance3D(nextPoint, currPoint);
                    allDistances.append(distance);
                    stats.maxDistance = std::max(stats.maxDistance, distance);
                }
            }
        }
    }

    // 计算距离统计
    if (!allDistances.isEmpty()) {
        float sum = std::accumulate(allDistances.begin(), allDistances.end(), 0.0f);
        stats.avgDistance = sum / allDistances.size();

        float variance = 0.0f;
        for (float dist : allDistances) {
            variance += std::pow(dist - stats.avgDistance, 2);
        }
        stats.stdDistance = std::sqrt(variance / allDistances.size());
        stats.distanceCount = allDistances.size();
    }

    return stats;
}

ContourData ContourProcessor::processContours(const ContourData& inputData,
                                              const ProcessingParams& params) {
    // 重置统计
    m_processingStats = ProcessingStats();

    // 计算全局统计
    m_globalStats = calculateGlobalStats(inputData, params.interpMode);

    ContourData processedData;
    processedData.reserve(inputData.size());

    // 处理每个切片
    for (const Slice& slice : inputData) {
        Slice processedSlice;
        processedSlice.reserve(slice.size());

        ProcessingStats sliceStats;

        // 处理每个轮廓
        for (const QVector<Vertex>& contour : slice) {
            QVector<Vertex> processedContour = processSingleContour(contour, m_globalStats, params, sliceStats);
            processedSlice.append(processedContour);
        }

        processedData.append(processedSlice);

        // 累加切片统计
        m_processingStats.totalOriginalPoints += sliceStats.totalOriginalPoints;
        m_processingStats.totalProcessedPoints += sliceStats.totalProcessedPoints;
        m_processingStats.totalLowerRemoved += sliceStats.totalLowerRemoved;
        m_processingStats.totalHigherAdded += sliceStats.totalHigherAdded;
        m_processingStats.originalLowerPoints += sliceStats.originalLowerPoints;
        m_processingStats.originalHigherPoints += sliceStats.originalHigherPoints;
    }

    // 打印统计结果
    // qDebug() << "\n16位处理完成（模式：" << static_cast<int>(params.interpMode) << "）：";
    // qDebug() << "  强度阈值：" << params.binaryThreshold << "/" << GRAY16_MAX;
    // qDebug() << "  点数量统计：";
    // qDebug() << "    原始总点数：" << m_processingStats.totalOriginalPoints;
    // qDebug() << "    处理后总点数：" << m_processingStats.totalProcessedPoints;
    // qDebug() << "    低强度点（<" << params.binaryThreshold << "）：原始"
    //          << m_processingStats.originalLowerPoints << "→ 剔除"
    //          << m_processingStats.totalLowerRemoved << "→ 剩余"
    //          << (m_processingStats.originalLowerPoints - m_processingStats.totalLowerRemoved);
    // qDebug() << "    高强度点（≥" << params.binaryThreshold << "）：原始"
    //          << m_processingStats.originalHigherPoints << "→ 新增"
    //          << m_processingStats.totalHigherAdded << "→ 剩余"
    //          << (m_processingStats.originalHigherPoints + m_processingStats.totalHigherAdded);
    // qDebug() << "    净变化：" << (m_processingStats.totalProcessedPoints - m_processingStats.totalOriginalPoints);

    return processedData;
}

VertexConversionResult ContourProcessor::convertToFlatVertices(const ContourData &processedData)
{
    VertexConversionResult result;

    if (processedData.isEmpty()) {
        return result;
    }

    result.totalSlices = processedData.size();

    // 预分配内存以提高性能
    int totalVertices = 0;
    for (const Slice& slice : processedData) {
        result.totalContours += slice.size();
        for (const QVector<Vertex>& contour : slice) {
            totalVertices += contour.size();
        }
    }

    result.vertices.reserve(totalVertices);
    result.sliceStartIndices.reserve(processedData.size());
    result.contourStartIndices.reserve(result.totalContours);

    // 展平数据并记录索引
    for (const Slice& slice : processedData) {
        result.sliceStartIndices.append(result.vertices.size());

        for (const QVector<Vertex>& contour : slice) {
            result.contourStartIndices.append(result.vertices.size());

            // 添加轮廓的所有顶点
            for (const Vertex& vertex : contour) {
                result.vertices.append(vertex);
            }
        }
    }

    return result;
}

QVector<QVector<QVector<QPointF>>> ContourProcessor::convertToStacksFormation(const ContourData &processedData, const QVector3D& u, const QVector3D& v)
{
    QVector<QVector<QVector<QPointF>>> result(processedData.size());

    std::for_each(std::execution::par, result.begin(), result.end(),
                  [&](auto& sliceResult) {
                      int sliceIndex = &sliceResult - &result[0];
                      const Slice& slice = processedData[sliceIndex];

                      QVector<QVector<QPointF>> sliceContours;
                      sliceContours.reserve(slice.size());

                      for (const QVector<Vertex>& contour : slice) {
                          QVector<QPointF> contourPoints;
                          contourPoints.reserve(contour.size());

                          for (const Vertex& vertex : contour) {
                              float u_coord = QVector3D::dotProduct(vertex.pos, u);
                              float v_coord = QVector3D::dotProduct(vertex.pos, v);
                              contourPoints.append(QPointF(u_coord, v_coord));
                          }

                          sliceContours.append(contourPoints);
                      }

                      sliceResult = std::move(sliceContours);
                  }
                  );

    return result;
}

} // namespace ContourProcessing
